#include <stdio.h>
#include <math.h>
#include "ParkingBlaze1.h"


/***********************************************/
/* Common functions for defuzzification methods*/
/***********************************************/

static double center(MembershipFunction mf) {
 return mf.center(mf);
}

/***********************************************/
/*  Common functions to create fuzzy numbers   */
/***********************************************/

static double MF_xfl_singleton_equal(MembershipFunction _mf,double x);

static FuzzyNumber createCrispNumber(double value) {
 FuzzyNumber fn;
 fn.crisp = 1;
 fn.crispvalue = value;
 fn.length = 0;
 fn.inputlength = 0;
 return fn;
}

static FuzzyNumber createFuzzyNumber(int length, int inputlength,
                                     double*input, OperatorSet op) {
 int i;
 FuzzyNumber fn;
 fn.crisp = 0;
 fn.crispvalue = 0;
 fn.length = length;
 for(i=0; i<length; i++) fn.degree[i] = 0;
 fn.inputlength = inputlength;
 fn.op = op;
 return fn;
}

/***********************************************/
/*  Functions to compute single propositions   */
/***********************************************/

static double _isEqual(MembershipFunction mf, FuzzyNumber fn) {
 return mf.compute_eq(mf,fn.crispvalue);
}

/***************************************/
/*  MembershipFunction MF_xfl_triangle  */
/***************************************/

static double MF_xfl_triangle_equal(MembershipFunction _mf,double x) {
 double a = _mf.param[0];
 double b = _mf.param[1];
 double c = _mf.param[2];
 return ( (a-x)<0 && (x-b)<=0? (x-a)/(b-a) : ((b-x)<0 && (x-c)<0? (c-x)/(c-b) : 0)); 
}

static MembershipFunction createMF_xfl_triangle( double min, double max, double step, double *param,int length) {
 int i;
 MembershipFunction _mf;
 _mf.min = min;
 _mf.max = max;
 _mf.step = step;
 for(i=0;i<length;i++) _mf.param[i] = param[i];
 _mf.compute_eq = MF_xfl_triangle_equal;
 _mf.compute_greq = NULL;
 _mf.compute_smeq = NULL;
 _mf.center = NULL;
 _mf.basis = NULL;
 return _mf;
}

/***************************************/
/*  MembershipFunction MF_xfl_singleton  */
/***************************************/

static double MF_xfl_singleton_center(MembershipFunction _mf) {
 double a = _mf.param[0];
  return a; 
}

static MembershipFunction createMF_xfl_singleton( double min, double max, double step, double *param,int length) {
 int i;
 MembershipFunction _mf;
 _mf.min = min;
 _mf.max = max;
 _mf.step = step;
 for(i=0;i<length;i++) _mf.param[i] = param[i];
 _mf.compute_eq = NULL;
 _mf.compute_greq = NULL;
 _mf.compute_smeq = NULL;
 _mf.center = MF_xfl_singleton_center;
 _mf.basis = NULL;
 return _mf;
}
/***************************************/
/*  Operatorset OP_opset */
/***************************************/

static double OP_opset_And(double a, double b) {
  return a*b; 
}

static double OP_opset_Defuz(FuzzyNumber mf) {
 double min = mf.conc[0].min;
 double max = mf.conc[0].max;
   double num=0, denom=0;
   int i;
   for(i=0; i<mf.length; i++) {
    num += mf.degree[i] * center(mf.conc[i]);
    denom += mf.degree[i];
   }
   if(denom==0) return (min+max)/2;
   return num/denom;
}

static OperatorSet createOP_opset() {
 OperatorSet op;
 op.and = OP_opset_And;
 op.or = NULL;
 op.also = NULL;
 op.imp = NULL;
 op.not = NULL;
 op.very = NULL;
 op.moreorless = NULL;
 op.slightly = NULL;
 op.defuz = OP_opset_Defuz;
 return op;
}


/***************************************/
/*  Type TP_Tx */
/***************************************/

static TP_Tx createTP_Tx() {
 TP_Tx tp;
 double min = -10.0;
 double max = 10.0;
 double step = 0.0784313725490196;
 double _p_mf0[3] = { -13.3,-10.0,-5.0 };
 double _p_mf1[3] = { -10.0,-5.0,-2.2 };
 double _p_mf2[3] = { -5.0,-2.2,0.0 };
 double _p_mf3[3] = { -2.2,0.0,2.2 };
 double _p_mf4[3] = { 0.0,2.2,5.0 };
 double _p_mf5[3] = { 2.2,5.0,10.0 };
 double _p_mf6[3] = { 5.0,10.0,13.3 };
 tp.mf0 = createMF_xfl_triangle(min,max,step,_p_mf0,3);
 tp.mf1 = createMF_xfl_triangle(min,max,step,_p_mf1,3);
 tp.mf2 = createMF_xfl_triangle(min,max,step,_p_mf2,3);
 tp.mf3 = createMF_xfl_triangle(min,max,step,_p_mf3,3);
 tp.mf4 = createMF_xfl_triangle(min,max,step,_p_mf4,3);
 tp.mf5 = createMF_xfl_triangle(min,max,step,_p_mf5,3);
 tp.mf6 = createMF_xfl_triangle(min,max,step,_p_mf6,3);
 return tp;
}

/***************************************/
/*  Type TP_Tangle */
/***************************************/

static TP_Tangle createTP_Tangle() {
 TP_Tangle tp;
 double min = -180.0;
 double max = 180.0;
 double step = 1.411764705882353;
 double _p_mf0[3] = { -240.0,-180.0,-130.0 };
 double _p_mf1[3] = { -180.0,-130.0,-50.0 };
 double _p_mf2[3] = { -130.0,-50.0,0.0 };
 double _p_mf3[3] = { -50.0,0.0,50.0 };
 double _p_mf4[3] = { 0.0,50.0,130.0 };
 double _p_mf5[3] = { 50.0,130.0,180.0 };
 double _p_mf6[3] = { 130.0,180.0,240.0 };
 tp.mf0 = createMF_xfl_triangle(min,max,step,_p_mf0,3);
 tp.mf1 = createMF_xfl_triangle(min,max,step,_p_mf1,3);
 tp.mf2 = createMF_xfl_triangle(min,max,step,_p_mf2,3);
 tp.mf3 = createMF_xfl_triangle(min,max,step,_p_mf3,3);
 tp.mf4 = createMF_xfl_triangle(min,max,step,_p_mf4,3);
 tp.mf5 = createMF_xfl_triangle(min,max,step,_p_mf5,3);
 tp.mf6 = createMF_xfl_triangle(min,max,step,_p_mf6,3);
 return tp;
}

/***************************************/
/*  Type TP_TWheel */
/***************************************/

static TP_TWheel createTP_TWheel() {
 TP_TWheel tp;
 double min = -0.4;
 double max = 0.4;
 double step = 0.0031372549019607846;
 double _p_mf0[1] = { -0.4 };
 double _p_mf1[1] = { 0.4 };
 double _p_mf2[1] = { 0.0 };
 tp.mf0 = createMF_xfl_singleton(min,max,step,_p_mf0,1);
 tp.mf1 = createMF_xfl_singleton(min,max,step,_p_mf1,1);
 tp.mf2 = createMF_xfl_singleton(min,max,step,_p_mf2,1);
 return tp;
}

/***************************************/
/*  Type TP_TY */
/***************************************/

static TP_TY createTP_TY() {
 TP_TY tp;
 double min = -2.0;
 double max = 30.0;
 double step = 0.12549019607843137;
 double _p_mf0[3] = { -18.0,-2.0,14.0 };
 double _p_mf1[3] = { -2.0,14.0,30.0 };
 double _p_mf2[3] = { 14.0,30.0,46.0 };
 tp.mf0 = createMF_xfl_triangle(min,max,step,_p_mf0,3);
 tp.mf1 = createMF_xfl_triangle(min,max,step,_p_mf1,3);
 tp.mf2 = createMF_xfl_triangle(min,max,step,_p_mf2,3);
 return tp;
}

/***************************************/
/*  Rulebase RL_backward */
/***************************************/

static void RL_backward(FuzzyNumber x, FuzzyNumber angle, FuzzyNumber *wheel) {
 OperatorSet _op = createOP_opset();
 double _rl, _output;
 int _i_wheel=0;
 TP_Tx _t_x = createTP_Tx();
 TP_Tangle _t_angle = createTP_Tangle();
 TP_TWheel _t_wheel = createTP_TWheel();
 double _input[2];
 _input[0] = x.crispvalue;
 _input[1] = angle.crispvalue;
 *wheel = createFuzzyNumber(49,2,_input,_op);
 _rl = _op.and(_isEqual(_t_x.mf0,x),_isEqual(_t_angle.mf0,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf0,x),_isEqual(_t_angle.mf1,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf0,x),_isEqual(_t_angle.mf2,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf0,x),_isEqual(_t_angle.mf3,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf0,x),_isEqual(_t_angle.mf4,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf0,x),_isEqual(_t_angle.mf5,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf0,x),_isEqual(_t_angle.mf6,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf1,x),_isEqual(_t_angle.mf0,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf1,x),_isEqual(_t_angle.mf1,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf1,x),_isEqual(_t_angle.mf2,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf1,x),_isEqual(_t_angle.mf3,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf1,x),_isEqual(_t_angle.mf4,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf1,x),_isEqual(_t_angle.mf5,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf1,x),_isEqual(_t_angle.mf6,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf2,x),_isEqual(_t_angle.mf0,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf2,x),_isEqual(_t_angle.mf1,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf2,x),_isEqual(_t_angle.mf2,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf2,x),_isEqual(_t_angle.mf3,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf2,x),_isEqual(_t_angle.mf4,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf2,x),_isEqual(_t_angle.mf5,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf2,x),_isEqual(_t_angle.mf6,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf3,x),_isEqual(_t_angle.mf0,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf3,x),_isEqual(_t_angle.mf1,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf3,x),_isEqual(_t_angle.mf2,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf3,x),_isEqual(_t_angle.mf3,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf2;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf3,x),_isEqual(_t_angle.mf4,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf3,x),_isEqual(_t_angle.mf5,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf3,x),_isEqual(_t_angle.mf6,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf4,x),_isEqual(_t_angle.mf0,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf4,x),_isEqual(_t_angle.mf1,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf4,x),_isEqual(_t_angle.mf2,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf4,x),_isEqual(_t_angle.mf3,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf4,x),_isEqual(_t_angle.mf4,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf4,x),_isEqual(_t_angle.mf5,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf4,x),_isEqual(_t_angle.mf6,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf5,x),_isEqual(_t_angle.mf0,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf5,x),_isEqual(_t_angle.mf1,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf5,x),_isEqual(_t_angle.mf2,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf5,x),_isEqual(_t_angle.mf3,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf5,x),_isEqual(_t_angle.mf4,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf5,x),_isEqual(_t_angle.mf5,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf5,x),_isEqual(_t_angle.mf6,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf6,x),_isEqual(_t_angle.mf0,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf6,x),_isEqual(_t_angle.mf1,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf6,x),_isEqual(_t_angle.mf2,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf6,x),_isEqual(_t_angle.mf3,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf6,x),_isEqual(_t_angle.mf4,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf0;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf6,x),_isEqual(_t_angle.mf5,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _rl = _op.and(_isEqual(_t_x.mf6,x),_isEqual(_t_angle.mf6,angle));
 wheel->degree[_i_wheel] = _rl;
 wheel->conc[_i_wheel] =  _t_wheel.mf1;
 _i_wheel++;
 _output = _op.defuz(*wheel);
 *wheel = createCrispNumber(_output);
}

/***************************************/
/*          Inference Engine           */
/***************************************/

void ParkingBlaze1InferenceEngine(double _d_x, double _d_angle, double *_d_wheel) {
 FuzzyNumber x = createCrispNumber(_d_x);
 FuzzyNumber angle = createCrispNumber(_d_angle);
 FuzzyNumber wheel;
 RL_backward(x, angle, &wheel);
 *_d_wheel = wheel.crispvalue;
}

